﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using WFNetLib;

namespace RobotControl
{
    public class 磁力仪
    {
        public int SensorsCount;
        public int MReadings;
        public Union_Int32[,] Uncompensated;
        public Union_Int16[,] FirstFourADChannels;
        public Union_Int16[,] RestOfADChannels;
        public byte[] SensorLevel;
        public Union_Int16 SynchStatus;
        public Union_Int16 AD8Value;
        public 磁力仪(int s,int m)
        {
            SensorsCount = s;
            MReadings = m;
            Uncompensated = new Union_Int32[s, m];
            FirstFourADChannels = new Union_Int16[s, m];
            RestOfADChannels = new Union_Int16[s, m];           
            NeedRx = 13 + 8 * m * s;
            SensorLevel = new byte[4];
            RxList = new byte[NeedRx];
        }
        int RxCount = 0;
        public int NeedRx;
        public byte[] RxList;
        public bool RxProc(byte rx)
        {
            RxList[RxCount++] = rx;
            switch (RxCount)
            {
                case 1:
                    if (rx != (byte)'$')
                        RxCount = 0;
                    break;
                case 2:
                    if (rx != (byte)'M')
                        RxCount = 0;
                    break;
                case 3:
                    if (rx != (byte)'M')
                        RxCount = 0;
                    break;
                case 4:
                    if (rx != (byte)'4')
                        RxCount = 0;
                    break;                
            }
            if (RxCount == NeedRx)
            {
                RxCount = 0;
                SensorLevel[0] = RxList[4];
                SensorLevel[1] = RxList[5];
                SensorLevel[2] = RxList[6];
                SensorLevel[3] = RxList[7];
                SynchStatus.ofs0_8 = RxList[8];
                SynchStatus.ofs1_8 = (sbyte)RxList[9];
                AD8Value.ofs0_8 = RxList[10];
                AD8Value.ofs1_8 = (sbyte)RxList[11];
                for (int i = 0; i < SensorsCount; i++)
                {
                    for (int j = 0; j < MReadings; j++)
                    {
                        Uncompensated[i, j].ofs0_16.ofs0_8 = RxList[12 + i * 4 * MReadings + j * 4];
                        Uncompensated[i, j].ofs0_16.ofs1_8 = RxList[12 + i * 4 * MReadings + j * 4 + 1];
                        Uncompensated[i, j].ofs1_16.ofs0_8 = RxList[12 + i * 4 * MReadings + j * 4 + 2];
                        Uncompensated[i, j].ofs1_16.ofs1_8 = (sbyte)RxList[12 + i * 4 * MReadings + j * 4 + 3];
                    }
                }
                for (int i = 0; i < SensorsCount; i++)
                {
                    for (int j = 0; j < MReadings; j++)
                    {
                        FirstFourADChannels[i, j].ofs0_8 = RxList[12 + 4 * SensorsCount * MReadings + i * 2 * MReadings + j * 2];
                        FirstFourADChannels[i, j].ofs1_8 = (sbyte)RxList[12 + 4 * SensorsCount * MReadings + i * 2 * MReadings + j * 2 + 1];
                    }
                }
                for (int i = 0; i < SensorsCount; i++)
                {
                    for (int j = 0; j < MReadings; j++)
                    {
                        RestOfADChannels[i, j].ofs0_8 = RxList[12 + 2 * SensorsCount * MReadings + 4 * SensorsCount * MReadings + i * 2 * MReadings + j * 2];
                        RestOfADChannels[i, j].ofs1_8 = (sbyte)RxList[12 + 2 * SensorsCount * MReadings + 4 * SensorsCount * MReadings + i * 2 * MReadings + j * 2 + 1];
                    }
                }                
                return true;
            }
            return false;
        }
    }
}
